#include "CoordConverter.h"
#include <osg/CoordinateSystemNode>

namespace FeAlg
{
    osg::Vec3d RadianLLH2XYZ(const osgEarth::MapNode* pMapNode, const osg::Vec3d& vecLLH)
    {
		osg::Vec3d vecXYZ;
		if (pMapNode)
		{
			pMapNode->getMapSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
					vecLLH.y(), vecLLH.x(), vecLLH.z(), vecXYZ.x(), vecXYZ.y(), vecXYZ.z());
		}
        return vecXYZ;
    }

    osg::Vec3d DegreeLLH2XYZ(const osgEarth::MapNode* pMapNode, const osg::Vec3d& vecLLH)
    {
		osg::Vec3d vecRadianLLH(osg::DegreesToRadians(vecLLH.x()), osg::DegreesToRadians(vecLLH.y()), vecLLH.z());
		return RadianLLH2XYZ(pMapNode, vecRadianLLH);
    }

    osg::Vec3d XYZ2RadianLLH(const osgEarth::MapNode* pMapNode, const osg::Vec3d& vecXYZ)
    {
		osg::Vec3d vecLLH;
		if (pMapNode)
		{
			pMapNode->getMapSRS()->getEllipsoid()->convertXYZToLatLongHeight(
				vecXYZ.x(), vecXYZ.y(), vecXYZ.z(), vecLLH.y(), vecLLH.x(), vecLLH.z());
		}
        return vecLLH;
    }

    osg::Vec3d XYZ2DegreeLLH(const osgEarth::MapNode* pMapNode, const osg::Vec3d& vecXYZ)
    {
		osg::Vec3d vecLLH = XYZ2RadianLLH(pMapNode, vecXYZ);
		vecLLH.x() = osg::RadiansToDegrees(vecLLH.x());
		vecLLH.y() = osg::RadiansToDegrees(vecLLH.y());
		return vecLLH;
    }

	void RadiaLLH2Matrix(const osgEarth::MapNode* pMapNode, const osg::Vec3d& vecLLH, osg::Matrix& matrix )
	{
		if (pMapNode)
		{
			pMapNode->getMapSRS()->getEllipsoid()->computeLocalToWorldTransformFromLatLongHeight(vecLLH.y(),vecLLH.x(),vecLLH.z(),matrix);
		}
	}

	void DegreeLLH2Matrix(const osgEarth::MapNode* pMapNode, const osg::Vec3d& vecLLH,osg::Matrix& matrix )
	{
		RadiaLLH2Matrix(pMapNode, osg::Vec3d(osg::DegreesToRadians(vecLLH.x()),osg::DegreesToRadians(vecLLH.y()),vecLLH.z()), matrix);
	}

	void XYZ2Matrix(const osgEarth::MapNode* pMapNode, const osg::Vec3d& vecXYZ,osg::Matrix& matrix )
	{
		osg::Vec3d vecLLHRadia = XYZ2RadianLLH(pMapNode, vecXYZ);
		RadiaLLH2Matrix(pMapNode, vecLLHRadia, matrix);
	}

	bool ScreenXY2DegreeLLH(const osgEarth::MapNode* pMapNode, osg::View* pView, float fX, float fY, osg::Vec3d& vecLLH)
	{		
		if(ScreenXY2RadiaLLH(pMapNode, pView, fX, fY, vecLLH))
		{
			vecLLH.x() = osg::RadiansToDegrees(vecLLH.x());
			vecLLH.y() = osg::RadiansToDegrees(vecLLH.y());
			return true;
		}

		return false;
	}

	bool ScreenXY2RadiaLLH(const osgEarth::MapNode* pMapNode, osg::View* pView, float fX, float fY, osg::Vec3d& vecLLH)
	{
		if(pMapNode && pView)
		{
			osg::Vec3d vecWorld;
			if(pMapNode->getTerrain()->getWorldCoordsUnderMouse(pView, fX, fY, vecWorld))
			{
				/*pMapNode->getMapSRS()->getEllipsoid()->convertXYZToLatLongHeight(
					vecWorld.x(), vecWorld.y(), vecWorld.z(), vecLLH.y(), vecLLH.x(), vecLLH.z());*/

				osgEarth::GeoPoint mapPoint;
				mapPoint.fromWorld( pMapNode->getMapSRS(), vecWorld);
				vecLLH = mapPoint.vec3d();

				return true;
			}
		}

		return false;
	}

}